package fem2.element;

import inf.jlinalg.Array1DMatrix;
import inf.jlinalg.BLAM;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IMatrixReadOnly;
import inf.jlinalg.IVector;
import inf.jlinalg.IVectorReadOnly;
import inf.math.LinAlg;
import math2.LinAlgUtilities;
import fem2.Debugger;
import fem2.MaterialModel;
import fem2.MeshPart;
import fem2.Node;
import fem2.NotImplementedError;
import fem2.enu.MassMatrixType;
import fem2.material.ESfemManager;

public class ESfemStructuralElement extends StructuralElement {

	public ESfemStructuralElement(MeshPart mp, MaterialModel mm) {
		super(mp, mm);
		int dim = mp.getDimension();
		assert dim == 2 : "this element only supports 2D mesh part (face)";
		assert mm instanceof ESfemManager : "this element must be coupled with ESfemManager";
	}

	@Override
	public void computeB(MeshPart mp, IMatrix b, double... xi) {
		int dim = mp.getDimension();
		int n = mp.getBasis().getDimension();
		Node[] nodes = mp.getNodes();
		double[] c = mp.computeCenter();

		int[] ids = new int[] { (int) xi[0], (int) xi[1] };
		double Ac = ((ESfemManager) getMaterialModel()).getSmoothingDomainArea(nodes[ids[0]],
				nodes[ids[1]]);
		// Debugger.watch("Ac = ", Ac);

		for (int i = 0; i < 2; i++) {
			int k = ids[i];
			double[] p = nodes[k].getPosition();

			/*
			 * find the center of the edge
			 */
			double[] x = new double[] { (c[0] + p[0]) / 2, (c[1] + p[1]) / 2 };

			/*
			 * get the reference coordinate of that point
			 */
			xi = mp.XiAt(x);
			Debugger.watch("xi = ", xi);

			/*
			 * compute shape function values
			 */
			double[] values = mp.valueAt(xi);

			/*
			 * compute normal vector
			 */
			double[] normal = new double[] { -(c[1] - p[1]), (c[0] - p[0]) };
			double length = LinAlg.norm2(dim, normal);
			normal[0] /= length;
			normal[1] /= length;

			// Debugger.watch("c = ", c);
			// Debugger.watch("p = ", p);
			// Debugger.watch("length = ", length);

			int k1;
			if (i == 0) {
				k1 = ids[1];
			} else {
				k1 = ids[0];
			}
			double[] p1 = nodes[k1].getPosition();
			if ((p1[0] - c[0]) * normal[0] + (p1[1] - c[1]) * normal[1] > 0) {
				normal[0] = -normal[0];
				normal[1] = -normal[1];
			}

			Debugger.watch("element id = ", getId());
			Debugger.watch("ids = ", ids);
			Debugger.watch("normal = ", normal);
			// Debugger.watch("c = ", c);
			// Debugger.watch("p = ", p);

			/*
			 * add up to B matrix
			 */
			for (int j = 0; j < n; j++) {
				double tmp = values[j] * length / Ac;
				b.add(0, 2 * j, normal[0] * tmp);
				b.add(1, 2 * j + 1, normal[1] * tmp);
				b.add(2, 2 * j, normal[1] * tmp);
				b.add(2, 2 * j + 1, normal[0] * tmp);
			}
		}

		Debugger.watch("--------------");

	}

	@Override
	public IMatrixReadOnly computeKte(IMatrix ke) {
		MeshPart mp = getMeshPart();
		int n = mp.getBasis().getDimension();
		int dim = mp.getDimension();
		int ntrs = dim * (dim + 1) / 2;
		Array1DMatrix B = Array1DMatrix.createColumnMajor(ntrs, dim * n);
		double th = getMaterialModel().getSectionProperty();

		int ng = getNumIntegrationPoints();
		for (int k = 0; k < ng; k++) {
			double w = getIntegrationWeight(k);
			double[] xi = getIntegrationPoint(k);
			double coeff = w * th;

			BLAM.zero(B);
			computeB(mp, B, xi);

			IMatrix C = getMaterialModel().getConstitutiveMatrix(this, k);
			LinAlgUtilities.btCB(B, C, coeff, ke);
		}

		return ke;
	}

	@Override
	public IVectorReadOnly computeRie(IVector rie) {
		MeshPart mp = getMeshPart();
		int n = mp.getBasis().getDimension();
		int dim = mp.getDimension();
		int ntrs = dim * (dim + 1) / 2;
		Array1DMatrix B = Array1DMatrix.createColumnMajor(ntrs, dim * n);
		double th = getMaterialModel().getSectionProperty();

		int ng = getNumIntegrationPoints();
		for (int k = 0; k < ng; k++) {
			double w = getIntegrationWeight(k);
			double[] xi = getIntegrationPoint(k);
			double coeff = w * th;

			BLAM.zero(B);
			computeB(mp, B, xi);

			IVector s = getStress(k);
			BLAM.multiply(coeff, BLAM.TRANSPOSE, B, s, 1.0, rie);
		}

		return rie;
	}

	@Override
	public IVectorReadOnly computeRes(IVector res) {
		/*
		 * TODO
		 */
		throw new NotImplementedError();
	}

	@Override
	public IMatrixReadOnly computeMe(IMatrix me, MassMatrixType massType) {
		/*
		 * TODO
		 */
		throw new NotImplementedError();
	}

	@Override
	public IMatrixReadOnly computeKge(IMatrix ke) {
		/*
		 * TODO
		 */
		throw new NotImplementedError();
	}

	@Override
	public int getNumIntegrationPoints() {
		return getMeshPart().getNodes().length;
	}

	@Override
	public double[] getIntegrationPoint(int k) {
		if (k < getMeshPart().getNodes().length - 1) {
			return new double[] { k, k + 1 };
		} else {
			return new double[] { k, 0 };
		}
	}

	@Override
	public double getIntegrationWeight(int k) {
		ESfemManager mm = (ESfemManager) getMaterialModel();
		Node[] nodes = getMeshPart().getNodes();
		if (k < getMeshPart().getNodes().length - 1) {
			return mm.getSmoothingDomainArea(nodes[k], nodes[k + 1]);
		} else {
			return mm.getSmoothingDomainArea(nodes[k], nodes[0]);
		}
	}

}

// @Override
// public void computeB(MeshPart mp, IMatrix b, double... xi) {
// int dim = mp.getDimension();
// int n = mp.getBasis().getDimension();
// double Ac = 0.0;
//
// int i1 = (int) xi[0];
// int i2 = (int) xi[1];
//
// double[][] p = new double[][] { mp.getNodes()[i1].getPosition(),
// mp.getNodes()[i1].getPosition(), mp.computeCenter() };
//
// for (int i = 0; i < 3; i++) {
// i1 = i;
// if (i < 2) {
// i2 = i + 1;
// } else {
// i2 = 0;
// }
//
// /*
// * find the center of the edge
// */
// double[] x = new double[] { (p[i1][0] + p[i2][0]) / 2, (p[i1][1] + p[i2][1])
// / 2 };
//
// /*
// * get the reference coordinate of that point
// */
// xi = mp.XiAt(x);
//
// /*
// * compute shape function values
// */
// double[] values = mp.valueAt(xi);
//
// /*
// * compute normal vector
// */
// double[] normal = new double[] { -(p[i2][1] - p[i1][1]), (p[i2][0] -
// p[i1][0]) };
// double length = LinAlg.norm2(dim, x);
// normal[0] /= length;
// normal[1] /= length;
//
// /*
// * ensure normal vector pointing utward
// */
// int k1;
// if (i == 0) {
// k1 = (int) xi[1];
// } else {
// k1 = (int) xi[0];
// }
// double[] p1 = mp.getNodes()[k1].getPosition();
// if ((p1[0] - c[0]) * normal[0] + (p1[1] - c[1]) * normal[1] > 0) {
// normal[0] = -normal[0];
// normal[1] = -normal[1];
// }
//
// /*
// * add up to B matrix
// */
// for (int j = 0; j < n; j++) {
// double tmp = values[j] * length / Ac;
// b.add(0, 2 * j, normal[0] * tmp);
// b.add(1, 2 * j + 1, normal[1] * tmp);
// b.add(2, 2 * j, normal[1] * tmp);
// b.add(2, 2 * j + 1, normal[0] * tmp);
// }
// }
//
// }